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Abstract 

This  paper  studies  the  problem  of  selective  stabilization,  defined  here  as  the  separation  of 
rotation  components  due  to  smooth  rotation  and  oscillatory  rotation.  In  applications  to  off-road 
navigation,  in  addition  to  the  desired  smooth  attitude  change,  a  vehicle  undergoes  high  frequency 
vibration.  We  consider  both  kinematic  and  kinetic  models  suitable  for  capturing  these  phenomena 
and  achieving  their  separation.  Our  approach  uses  various  dynamic  laws  to  model  the  behavior 
of  the  vehicle,  and  relies  on  Extended  Kalman  Filters  for  the  estimation.  Multiple  visual  cues 
are  exploited  to  obtain  the  rotational  parameters.  A  scheme  for  detecting  the  occurrence  and 
disappearance  of  smooth  rotation  is  devised.  Appropriate  dynamic  laws  are  employed  to  achieve 
selective  stabilization.  Based  on  the  selective  stabilization,  3-D  locations  of  close  feature  points 
are  estimated  in  a  stabilized  frame  of  reference,  thus  providing  more  useful  information.  Synthetic 
experiments  for  different  scenarios  using  the  proposed  approach  show  promising  results. 

Keywords:  Selective  stabilization,  motion  analysis,  structure  from  selective  stabilization,  kine¬ 
matic  and  kinetic  models 
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1  Introduction 


Vision  has  been  an  important  factor  in  intelligent  mobile  robotics.  In  order  to  move  around  the 
environment,  knowledge  about  a  robot’s  motion  and  relevant  structures  in  the  environment  is 
essential.  Many  related  tasks  such  as  moving  object  detection,  obstacle  avoidance,  and  target 
acquisition  also  become  easier  when  this  information  is  available.  Numerous  approaches  to  these 
problems  have  been  proposed.  Depending  on  their  inputs,  major  techniques  are  typically  either 
flow-based  or  feature-based. 

Under  the  assumption  that  the  optical  flow  is  identical  to  the  motion  field,  flow-based  meth¬ 
ods  employ  spatial  and  temporal  image  gradients  to  infer  motion  and  structure.  Since  only  flow 
components  in  the  directions  of  image  gradients  can  be  computed  directly,  additional  constraints 
are  introduced  in  recovering  the  optical  flow  [8,  10,  11,  16].  Then,  based  on  the  computed  optical 
flow,  motion  and  structure  are  extracted  [1,  14,  19].  Alternatively,  direct  estimation  of  motion 
and  structure,  without  computing  the  optical  flow,  have  also  been  pursued  [9,  17].  Although  great 
advances  have  been  made,  instability  and  sensitivity  to  noise  in  estimating  image  derivatives  limit 
flow-based  methods  to  application  only  in  situations  where  the  image  sequence  is  dense,  while  the 
inter- frame  motion  is  also  small. 

Feature-based  techniques  use  a  set  of  discrete  features  such  as  points  or  lines  to  extract  infor¬ 
mation.  As  in  many  flow-based  approaches,  the  first  step  is  to  solve  a  correspondence  problem, 
i.e.  to  determine  corresponding  features  over  the  sequence.  The  motion  and  structure  are  then 
recovered  in  the  next  step.  Much  of  the  early  work  using  this  approach  focused  on  using  two  or 
three  frames,  resulting  in  simple  linear  algorithms  [13,  18].  However,  a  lack  of  robustness  of  the 
estimates  to  correspondence  errors  has  been  observed  [2,  6].  One  way  in  which  robust  algorithms 
can  be  obtained  is  by  exploiting  the  dynamic  nature  of  the  sequence. 

We  are  interested  in  studying  dynamic  laws  suitable  for  capturing  the  behavior  of  mobile  robots, 
especially  for  off-road  navigation  apphcations.  Since  the  translation  of  a  vehicle  is  mainly  along 
its  longitudinal  axis  and  its  speed  can  easily  be  obtained  from  the  odometer,  we  concentrate  on 
dynamic  laws  for  describing  the  rotation  of  a  vehicle.  The  orientation  of  a  vehicle,  in  general, 
is  determined  by  a  desired  smooth  rotation  and  an  undesired  high  frequency  oscillatory  rotation. 
The  smooth  rotation  is  defined  here  as  the  attitude  change  of  the  vehicle  arising  from  the  steering 
commands  and/or  the  effect  of  following  a  smooth  surface.  The  oscillatory  rotation,  on  the  other 
hand,  is  typically  the  response  of  mechanical  elements  to  roughness  components  in  natural  terrain. 
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In  applications  such  as  teleoperation,  it  is  desirable  to  provide  the  impression  of  smooth  rotation 
(such  as  arises  in  steering  and  climbing)  to  the  teleoperator,  while  removing  non-smooth  rotation. 
Additional  sensors  such  as  gyros  and  accelerometers  could  be  used  to  compensate  for  these  effects. 
However,  this  could  result  in  a  prohibitively  costly  system.  The  possibility  of  separating  the  two 
types  of  rotations  directly  from  the  images  is  therefore  of  interest.  We  refer  to  this  procedure  as 

selective  stabilization. 

To  achieve  selective  stabilization,  we  first  consider  the  recovery  of  parameters  relevant  to  the 
total  rotation,  which  is  the  sum  of  smooth  rotation  and  oscillatory  rotation.  This  process  is  called 
image  stabilization.  Image  plane  displacements  of  distant  feature  points  may  unambiguously  char¬ 
acterize  total  rotational  motion.  However,  such  points  are  sometimes  difficult  to  detect  and  track, 
due  to  the  absence  of  sufficient  intensity  gradient  information.  Horizon  lines,  when  present,  consti¬ 
tute  very  strong  visual  cues,  requiring  relatively  simple  operations  for  their  tracking.  These  tokens 
are  therefore  used  to  estimate  the  total  rotation  parameters. 

We  then  seek  to  separate  smooth  rotation  and  oscillatory  rotation.  Assuming  that  the  attitude  of 
the  vehicle  varies  smoothly,  a  kinematic  law  is  employed  to  model  the  behavior  of  the  corresponding 
smooth  rotation  parameters.  The  osciUatory  rotation  is  captured  by  a  kinetic  model.  A  four-wheel 
vehicle  model  used  in  the  design  of  suspension  systems  is  considered  for  this  purpose.  In  some 
situations,  parts  of  the  smooth  rotation  and  osciUatory  rotation  are  not  coupled.  For  example, 
if  high  frequency  yaw  motion  is  negfigible,  steering  directly  corresponds  to  a  component  of  total 
rotation.  Similarly,  if  the  banking  behavior  of  the  vehicle  is  not  considered,  roU  motion  is  directly 
observable  from  the  total  rotation.  Consequently,  only  the  climbing  and  pitch  motions  are  coupled. 
Furthermore,  climbing  does  not  occur  at  aU  times.  We  therefore  design  a  detection  scheme  for  the 
occurrence  and  disappearance  of  climbing.  Based  on  the  detection  results,  the  selective  stabilization 

algorithm  is  adjusted  accordingly. 

Another  issue  concerns  estimation  of  the  relative  positions  of  close  scene  points  with  respect  to 
the  vehicle.  Much  of  the  earlier  work  estimates  these  positions  without  considering  the  effect  of 
osciUatory  rotation.  Decisions  or  actions  based  on  the  resulting  estimates  may  be  inappropriate.  H 
selective  stabiUzation  can  be  achieved,  we  can  estimate  the  3-D  locations  of  a  set  of  close  features 
in  a  desirable  frame  of  reference,  which  is  less  perturbed  by  the  osciUatory  rotation. 

The  main  contributions  of  this  paper  are  as  foUows: 

•  Dynamic  models:  We  consider  the  osciUatory  behavior  of  a  vehicle  undergoing  off-road  nav- 
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igation.  A  kinetic  law  for  oscillatory  rotation  is  derived.  Its  performance  is  studied  both 
analytically  and  quantitatively. 

•  Selective  stabilization:  We  investigate  the  separability  of  smooth  rotation  and  oscillatory 
rotation.  Selective  removal  of  oscillatory  rotation  from  total  rotation  is  justified. 

•  Maneuver  detection:  Visual  cues  are  exploited  to  instantaneously  or  conservatively  detect  the 
occurrence  and  disappearance  of  smooth  rotation. 

•  Structure  from  selective  stabilization:  The  relative  positions  of  scene  points  are  estimated  in 
a  stabilized  frame  of  reference.  Impressions  of  steering  and  climbing  are  preserved. 

The  organization  of  this  paper  is  as  follows.  Section  2  discusses  the  observability  of  total  rotation 
from  visual  cues.  The  separability  of  smooth  rotation  and  oscillatory  rotation,  based  on  appropriate 
dynamic  laws,  is  addressed  in  Section  3.  The  complete  algorithm,  including  the  detection  of  the 
occurrence  and  disappearance  of  smooth  rotation  as  well  as  estimation  of  structure  parameters,  is 
presented  in  Section  4.  Section  5  reports  results  of  experiments  using  synthetic  data.  Conclusions 
are  given  in  Section  6. 

2  Observability  of  Full  Rotation 

This  section  addresses  the  observability  of  parameters  which  account  for  the  attitude  change  of 
the  vehicle  carrying  the  camera  [20].  In  particular,  we  address  parameter  recovery  from  points 
and  horizon  lines  in  an  image  sequence.  Consider  a  scenario  in  which  a  rigidly  mounted  camera 
undergoes  rotation  with  instantaneous  angular  velocity  w  :  and  translation  with 

linear  velocity  V  :  {V^,Vy,V^f.  Let  P  :  (X,Y,Z)'^  denote  the  3-D  position  of  a  scene  point  with 
respect  to  the  camera,  and  p  :  {x,yY‘  the  image  plane  coordinates  of  the  corresponding  projection 
point.  The  relative  motion  of  the  scene  point  with  respect  to  the  camera  is  then  described  by 

P  =  -wxP-V  (1) 

Assuming  that  perspective  projection  is  used  as  an  imaging  model,  p  is  related  to  P  as  follows: 

p=:P(P)+p,  (2) 
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where  Pc  is  the  intersection  of  the  optical  axis  with  the  image  plane  and  V  denotes  the  perspective 
projection  operator,  i.e. 


nf)  = 


f  i 

Jc  z 
Jcz 


(3) 


with  fc  the  focal  length.  Consequently,  the  image  motion  arising  from  the  camera  movement 

satisfies  ^ 

P  =  [/e~^(P  ~  ~  Pc)  +  '^^(P  ~  +  f<^^xy]  +  [(P  ~  Pfoe)-]  (4) 

Pr  Pt 

where  p/oe  =  ^(V)  +  Pc  and  r  =  Z/V^  respectively  correspond  to  the  Focus  of  Expansion  (FOE) 
and  the  time  to  collision  to  the  imaged  point;  is  the  2  x  2  skewed  symmetric  matrix  related  to 
the  rotational  velocity  component  along  the  optical  axis 


— 


(5) 


0  l^z 

-Wj  0 

and  is  an  image  vector  orthogonal  to  the  projection  of  the  instantaneous  angular  velocity 
component  parallel  to  the  image  plane, 


a? 


= 

xy 


(6) 


Consider  a  distant  point  (i.e.  let  r  oo);  denote  its  position  by  P.  As  seen  from  (4)  such 
points  move  relative  to  the  camera  as  if  only  rotation  were  present.  Therefore  we  may  assume  for 
these  points  that  the  3-D  motion  satisfies 


P  =  -UJ  X  P 


(7) 


For  off-road  vehicle  navigation,  or  images  taken  from  an  airplane  or  a  helicopter,  horizon  lines  or 
partial  profUes  of  distant  objects  constitute  very  strong  visual  cues.  In  Figure  1,  consider  an  image 
horizon  line  denoted  by  £;  £  is  uniquely  characterized  by  W,  the  3-D  vector  normal  to  the  plane  11 
through  £  and  the  camera  center.  Since  the  image  motion  of  horizon  lines  is  explained  exclusively 
in  terms  of  the  camera  rotation,  it  follows  that  the  motion  of  the  normal  vector  W  is  itself  described 

by 

W  =  -oj  X  W  (8) 


If  u  is  one  solution,  i.e.  W  =  -u  x  W,  then  fcW  +  u  also  satisfies  (8)  for  any  k.  Therefore, 
observing  the  image  motion  of  one  horizon  line  characterizes  the  rotational  component  on  plane 


4 


Figure  1:  Geometric  representation  of  a  horizon  line  C,  the  plane  II,  and  the  3-D  normal  vector 

W. 

n  only.  There  is  indeterminacy  along  the  direction  W.  Given  one  observation,  the  set  of  possible 
solutions  of  (8)  describes  an  afSne  line  in  3-D  rotational  parameter  space.  Indeterminacy  also 
exists  if  only  one  distant  point  is  observed.  In  this  case,  this  indeterminacy  involves  the  rotational 
component  along  a  ray  from  the  image  center  to  the  point. 

Quantitatively,  however,  lines  and  points  carry  equal  amounts  of  information  in  the  determi¬ 
nation  of  rotation.  If  any  combination  of  two  of  these  features  is  observed,  rotation  can  be  fuUy 
characterized,  except  in  some  degenerate  cases  for  which  W xP  =  0,  which  in  practice  cannot  occur 
unless  the  observer  possesses  an  unreasonably  large  Field  of  View  (FOV).  Letting  w  =  P(W)-|-Pc, 
one  may  solve  the  over-determined  linear  system 

Qu}  =  D  (9) 

where  D  =  [pf,  •  • , . . ., w^]^,  while  Q  is  a  matrix  derived  from  (4).  Line  features  can 
therefore  be  combined  with  other  tracked  tokens  such  as  distant  points  for  the  recovery  of  rotational 
pajameters. 

When  the  image  sequence  is  dense  and  the  inter-frame  motion  is  small,  (4)  effectively  describes 
the  image  motion.  However,  irrespective  of  the  particular  trajectories  of  the  vectors  aj(t)  and  V(t), 
the  positions  of  3-D  points  at  two  time  instants  can  always  be  described  by  an  element  of  the  Special 
Euclidean  group  SE{3)  (uniquely,  if  the  rotation  center  is  given),  i.e.,  there  exists  a  total  rotation 
R  :  [rij],i  =  1, . . ., 3,y  =  1, . . ., 3  and  translation  T  :  {T^,Ty,T^Y ,  between  any  two  frames,  such 


that  the  3-D  point  positions  Pi  and  P2  expressed  in  the  camera  frame  of  reference  satisfy 


P2  =  RPi  -1-  T 


(10) 


As  before,  for  a  distant  point,  the  contribution  from  the  translation  T  is  negligible.  The  image 
plane  positions  of  such  a  point  at  ti  and  t2  are  then  expressed  as 

P2  =  (c^Pi  +  l)”^(Api  +  b)  (11) 


where 


A  =  d  ^ 

/c^ll  +  ^0^31  fcfu  +  ^0^32 

/c’'21  +  ?/c^31  /c’"22  +  yc‘>'32 

011  012 

021  O22 

cr 

II 

! 

fcyc'^X2  d”  fc  ^13 

fc^c'^21  fcyc‘t'22  d"  fc  ^23 

-  Xct/c^32  d-  fcXc'r33 

Xcycf31  -  yl‘r32  d-  fcyc'1'33 

= 

h 

_h 

(12) 

(13) 


- 

’ 

f31 

Cl 

r32 

C2 

d  =  -XcT-ai  -  J/c^32  +  /c^33 


(14) 

(15) 


Similarly,  for  a  distant  line  feature  characterized  by  the  projected  normal  vector  w, 

W2  =  (c^wi  -f  l)"^(Awi  -|-  b)  (16) 


As  seen  from  (11)  and  (16),  for  distant  features,  their  image  plane  motions  are  described  by  pro¬ 
jective  group  operations.  This  is  expected  since  points  on  the  horizon  fall  into  a  plane  at  infinity 
and  the  image  motion  of  a  plane  is  described  exactly  by  a  projectivity  [1]. 

In  summary,  by  observing  the  image  motions  of  distant  features,  the  attitude  change  between 
two  frames  can  be  estimated.  Equivalently,  we  know  the  total  rotation  arising  from  steering  and 
climbing,  as  well  as  pitch  and  roU,  of  a  vehicle. 


3  Selective  Stabilization 

The  next  issue  concerns  the  separation  of  smooth  rotation  and  high  frequency  oscillatory  rotation. 
This  procedure  is  important  in  many  applications,  and  is  referred  to  here  as  selective  stabilization} 
For  an  active  vision  system,  the  separation  of  oscillatory  rotation  from  smooth  rotation  is  useful  for 

^Note  that  we  use  the  term  selective  stabilizationm  a  different  sense  than  [3]. 
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acMeving  fixation.  Another  example  where  selective  stabilization  is  important  is  teleoperation,  in 
which  the  vehicle  needs  to  be  remotely  controlled.  Information  such  as  the  3-D  locations  of  scene 
points  or  the  knowledge  of  the  vehicle’s  movement,  unperturbed  by  the  oscillatory  rotation,  while 
still  preserving  smooth  rotation,  is  highly  desirable;  the  teleoperator  needs  to  evaluate  the  effects 
of  climbing  and  steering.  In  addition,  while  an  approximate  selective  stabilization  scheme  could 
be  attempted  by  simple  low-pass  filtering  of  the  computed  total  rotational  components,  a  scheme 
which  closely  resembles  mechanical  stabilization  is  of  greater  interest. 

As  discussed  earlier,  the  temporal  information  present  in  the  sequence  should  be  exploited.  In 
addition,  suitable  dynamic  laws  also  facilitate  selective  stabilization. 

For  convenience,  denote  the  angular  velocities  arising  from  the  smooth  rotation  the  oscillatory 
rotation  by  and  oJusj  respectively.  Consider  first.  Since  the  smooth  rotation  is  due  to 
steering  and  climbing,  it  is  assumed  that  the  resulting  attitude  changes  smoothly  over  time.  A 
simple  kinematic  law  is  therefore  employed  to  describe  the  behavior  of  Wj:  {uJsx,^sy,<^sz)'^ ■, 

io,  =  0  (17) 

On  the  other  hand,  the  interactions  between  mechanical  elements  and  the  environment  result 
in  undesired  oscillatory  rotation.  We  employ  a  four-wheel  vehicle  model  to  account  for  oscillatory 
vehicular  movement.  This  model  takes  into  account  the  phenomena  of  bounce,  pitch  and  roll 
(illustrated  in  Figure  2),  and  has  been  widely  used  for  the  design  and  analysis  of  suspension  systems. 
Tires  are  modeled  by  linear  springs  with  the  same  stiffness  coefficient  Kt~  and  M^r  represent 
the  masses  of  unsprung  elements  such  as  the  front  and  reax  wheels  and  their  axles.  AT/,  Cf,  Kr  and 
Cr  are  the  characteristics  of  the  linear  springs  and  shock  absorbers  modeling  the  suspension  system. 
Assuming  that  each  tire  contacts  the  terrain  at  aU  times,  there  exist  seven  degrees  of  freedom:  the 
movements  of  unsprung  elements  {a:i,a:3,X5,a;7}  (shown  in  Figure  2),  the  bouncing  displacement 
of  the  center  of  gravity  of  the  sprung  element  Xc,  the  pitch  angle  6  and  the  roE  angle  (illustrated 
in  Figure  3,  in  which  Wa  and  Wb  constitute  the  wheel  base,  while  Ts,t  and  Ts,i  are  the  suspension 
tracks).  Their  behavior  is  described  in  detail  in  Appendix  A.  Consequently,  if  we  define  :x.us  to  be 
the  vector  containing  the  unstabilized  rotational  components, 

^us^=  (18) 

then  X.US  satisfies 

^US  ~  ^US^US  T  FujU,jS  (19) 
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where  u^s  includes  other  dependent  quantities, 


(20) 

with  X6  =  {xc,Xcf  and  x,,  =  {xi,x^,X3,X3,X5,X5,X7,X7f .  ^us  and  respectively  denote 
constant  matrices  whose  entries  are  related  to  system  parameters  such  as  the  mass  of  the  sprung 
element,  the  moment  of  inertia,  the  spring  characteristics,  etc.  Since  additional  sensors  such  as 
accelerometers  are  required  for  obtaining  ,  we  instead  approximate  the  behavior  of  x^s  by 

—  us  (21) 

It  is  shown  in  Appendix  B  that  this  approximation  introduces  an  additional  error  in  separating 
smooth  rotation  and  oscillatory  rotation.  However,  the  error  remains  bounded. 


Figure  2:  The  four-wheel  vehicle  model  [7]:  {a;oi>*  =  denotes  the  disturbances  from  the 

terrain;  {xi,X3,X5,X7}  represent  the  displacements  of  the  unsprung  elements;  and  {x2,X4,xe,xs} 
are  the  movements  of  the  four  corners  to  which  the  suspension  system  is  connected. 


A 


Figure  3;  The  relationship  between  the  stabilized  coordinate  system  V®  and  the  oscillatory  coordi¬ 
nate  system  V;  the  bounce  Xc,  the  pitch  angle  9,  and  the  roll  angle  (j). 
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We  now  address  the  separation  of  smooth  rotation  and  oscillatory  rotation  using  the  dynamic 
law  in  (21).  Consider  Figure  3.  Define  the  stabilized  coordinate  system  Vs{t)  whose  orientation 
is  determined  by  the  smooth  rotation,  and  whose  center  is  located  at  the  center  of  gravity  of  the 
vehicle.  Consider  also  the  unstabihzed  coordinate  system  V(f),  whose  attitude  is  in  addition  affected 
by  the  oscillatory  rotation,  and  with  its  origin  shifted  from  the  center  of  gravity  due  to  bounce. 
Then,  as  argued  in  the  previous  section,  from  two  images  acquired  at  times  t  —  At  and  t,  we  observe 
the  total  angular  velocity  accounting  for  the  net  attitude  change  between  V(f  -  At)  and  V(t).  In 
other  words,  if  —  At)  aligns  the  coordinate  system  V(t  —  At)  with  V(t),  Rs(cA>s;t,t  —  At) 

takes  into  account  the  attitude  difference  between  Vs(t  —  At)  and  Vs(t),  and  Rui(0,  ^;t)  accounts 
for  the  additional  attitude  due  to  the  oscillatory  rotation  at  time  t, 

1  <l>{t)  -o{t) 

B^,{9,(f>-,t)=  -<f>{t)  1  0  (22) 

e{t)  0  1 

then  it  can  be  shown  that 

R(w;  t,t  -  At)  =  Rus{0, 4>\  i)Rs(u^s;  t,  t  -  At)R-^(0,  <i>\  t  -  At)  (23) 

Subsequently,  assume  that  R  and  R*  can  be  approximated  as 

R(a>;t,f-At)  «  I-/2Af  (24) 

Iis{u}s-,t,t- At)  «  I-^sAt  (25) 


0 

-<^z 

Uy 

0 

^LOsz  ^sy 

0 

—LOx 

12s  =  A)sz 

0  ^sx 

_  -Wj, 

0 

1 - 

1 

COsx  ^ 

At  ~  (27) 

Then  the  total  angular  velocity  w  is  related  to  the  angular  velocity  due  to  the  smooth  rotation  and 
oscillatory  rotation  by 

w  =  Wj  +  u^us  (28) 


U,us  =  iOjA)^ 
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Since  the  angular  velocity  component  due  to  the  yaw  motion  is  assumed  to  be  zero,  we  only  focus 
on  the  separability  of  the  other  two  phenomena,  i.e.  climbing  and  pitch  as  well  as  banking  and  roll. 
Using  the  dynamic  laws  in  (17)  and  (21),  the  following  linear  system  can  be  constructed: 

xi  =  Gxi 
u>i  =  Fxi 

rp  .rp  . 

where  Xi  =  , 

02x2  02x4 

04x2  ^  us 

with  02x2,02x4  and  04x2  being  zero  matrices  of  conformable  dimensions, 

100100 

F  =  (32) 

0  1  0  0  0  1 

The  separability  of  smooth  rotation  and  oscillatory  rotation  can  now  be  verified  by  checking  the 
rank  of  the  observability  matrix  O, 

F 

FG 

FG® 

Except  in  some  special  cases  in  which  the  matrix  is  singular,  O  in  general  has  full  rank. 
Therefore,  by  employing  different  dynamic  laws  to  describe  the  smooth  rotation  and  oscillatory 
rotation,  it  is  possible  to  separate  the  two  rotation  phenomena.  The  next  section  presents  schemes 
for  the  estimation  of  total  angular  velocity,  the  detection  of  the  occurrence  and  disappearance  of 
smooth  rotation,  and  the  3-D  location  of  close  feature  points  in  the  stabilized  coordinate  system. 

4  Parameter  Estimation 

The  observabiUty  of  total  rotation  from  distant  features  indicates  that  the  recovery  of  the  total 
angular  velocity  can  be  achieved  using  an  LS  solution  in  (9).  However,  the  estimation  benefits  if 
temporal  information  is  exploited.  In  addition,  the  incorporation  of  appropriate  dynamic  laws  to 
describe  smooth  rotation  and  oscillatory  rotation  shows  the  possibility  of  selective  stabilization. 
Nonetheless,  since  smooth  rotation  does  not  occur  at  all  times,  awareness  of  the  occurrence  or 
disappearance  of  smooth  rotation,  especially  climbing,  facilitates  the  task.  This  section  describes 
various  techniques  which  constitute  our  algorithm  for  off-road  navigation. 
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4.1  Image  Stabilization 


Because  of  the  desirable  properties  of  recursive  estimators,  we  employ  an  Extended  Kalman  Filter 
(EKF)  to  estimate  the  total  angular  velocity  oj  [20].  The  state  vector  x  is  defined  as 

(34) 

To  exploit  temporal  information  for  robust  estimation,  a  simple  kinematic  law  is  used: 

X  =  0  (35) 


Subsequently, 


x(tj+i)  =  x(ti) 


(36) 


This  constitutes  the  plant  equation  in  our  recursive  estimation  algorithm.  We  turn  next  to  the 
observation  equations.  Assume  that  the  tracked  tokens  consist  of  M  points  (whose  projection 
points  are  pi, . . . , pm)  and  N  horizon  Unes  (each  with  associated  normal  vector  w,-,  i  =  1, . . . ,  N). 
The  measurement  vector  is  defined  by 


z  =  (pf,...,pLwf,...,w^)^ 

Then,  from  (11)  and  (16),  we  obtain  the  measurement  equations  as 

z(tj+i)  =  hi+iji[x(ti+i)]  +  n(ti-i-i) 


(37) 


(38) 


where  h  is  a  nonlinear  function  while  n  denotes  the  measurement  noise.  More  specifically, 
[x(tj+i)]  is  expressed  as 


(c^Pi  +  1)  ^(Api  +  b) 


ht+i,i[x(ti+i)]  = 


(c^PM  +  1)  ^(ApM  +  b) 
(c^wi  +  l)“^(Awi  +  b) 


(c^w^r  +  1)  ^( Awjv  +  b) 


(39) 


with  A,b,c  defined  in  (12)-(15). 

After  the  horizon  lines  and  points  are  tracked,  the  EKF  scheme  is  applied  to  estimate  the  total 
angular  velocity.  This  consists  of  the  following  steps: 
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•  step  1:  State  and  covariance  propagation: 


=  Htf)  (40) 

=  S(t+)  +  S^(t,-+i) 

where  x{if)  and  denote  the  estinaates  of  x(t,)  and  the  associated  covariances:  they 

are  obtained  based  on  information  contained  in  the  sequence  up  to  the  frame. 
and  on  the  other  hand,  are  the  predicted  estimates  of  x(ti+i)  and  the  predicted 

covariances,  respectively,  before  the  incorporation  of  the  (i  +  1)*^  frame,  while  is 

the  covariance  of  the  plant  noise  w(ti+i). 


•  Step  2:  State  and  covariance  update: 


K(ti+i)  —  j- +  Sn(ti+i)] 

Mif+i)  =  +  (41) 


where  x(t+n)  is  the  desired  estimate  of  x(ti+i),  and  is  the  associated  covariance. 

K(ti+i),  S:„(ti+i)  and  I  respectively  represent  the  gain  matrix,  the  covariance  of  n(ti+i),  and 
the  identity  matrix.  on  the  other  hand,  is  the  linearized  approximation  of 


dhi+i,i 


5x(ti+i) 


x(<.+i) 


(42) 


With  the  above  formulation,  we  now  proceed  to  describe  the  detection  scheme  which  facilitates 
selective  stabilization. 


4.2  Separation  of  Smooth  Rotation  and  Oscillatory  Rotation 

As  discussed  earlier,  when  the  high  frequency  yaw  motion  is  negligible,  steering  is  directly  known 
from  the  total  angular  velocity.  Similarly,  the  phenomenon  of  banking  only  lasts  for  short  periods, 
and  more  importantly,  it  has  no  effect  on  the  direction  in  which  the  vehicle  travels  later.  Banking 
is  therefore  also  neglected.  Consequently,  the  roll  motion  is  obtained  directly.  On  the  other  hand, 
climbing  is  not  negligible;  the  vehicle  generally  moves  along  a  direction  resulting  from  climbing. 
Thus,  the  separation  of  climbing  and  pitch  motions  is  important.  Like  the  other  types  of  smooth 
rotation,  climbing  only  lasts  for  short  periods.  Therefore,  we  design  a  scheme  to  detect  the  occur¬ 
rence  and  disappearance  of  climbing.  The  detection  results  are  then  used  to  adjust  the  selective 
stabihzation  scheme. 
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Recall  the  image  motion  expressed  in  (4).  When  a  distant  point  is  considered,  its  vertical 
movement  is  described  by 

X  =  Ux[{x  -  Xc){y  -  2/c)]  -  Wj,[/c  +  (»  -  Xcf]  +  Uz{y  -  yc)  (43) 

If  the  corresponding  projection  point  is  close  to  the  vertical  axis,  i.e.  2/  —  J/c  ~  0?  then 

X  -Uy[fc-\-{x  -  Xcf]  (44) 

In  other  words,  the  vertical  movements  of  such  features  are  dominantly  affected  by  climbing  and 
pitch  motions.  Since  the  previously  designed  scheme  assumes  a  zero  order  dynamic  law  for  the  total 
angular  velocity,  it  is  expected  that  when  the  vehicle  performs  maneuvers  (i.e.  starts  climbing  or 
finishes  climbing),  there  appear  large  errors  in  predicting  the  vertical  movements  of  corresponding 
points  at  these  instants.  Consequently,  we  define  [zm]x  to  be  the  vector  consisting  of  the  vertical 
coordinates  of  N  such  feature  points,  Pmi?  •  •  ^PmAT,  i-e- 

[Znijr  “  ([Pmllxj  •  •  •  >  [Pmiv]x)  (^^) 

and  the  test  statistic  S  at  time  t  as 

Sit)  {[Zrr^it)],  -  [Zm(t)].)^S-^(0([Zm(t)].  -  limit)].)  (46) 

where  z^it)  denotes  the  predicted  locations  of  these  N  feature  points  at  time  t;  it  is  computed 
according  to  (11),  in  which  the  projections  of  the  3-D  points  at  t  —  At  are  used  as  pi,  while  the 
predicted  value  of  total  angular  velocity  is  assumed  in  obtaining  A,  b,  c.  limit),  on  the  other  hand, 
is  the  covariance  matrix  of  the  innovation  vector  [zmit)].  ~  [zm(0]2;i  easily  be  computed  in 

our  EKF  formulation.  Based  on  Sit),  we  declare  that  the  vehicle  undergoes  maneuvers  at  t  -  At 
when 

Sit)  >  TH  (47) 

with  TH  a  threshold  value.  The  types  of  maneuvers  corresponding  to  the  occurrence  or  disappear¬ 
ance  of  climbing  can  then  be  determined.  If  the  vehicle  undergoes  climbing  at  a  rate  Ugyit)  much 
larger  than  the  pitching  velocity  0(t),  the  resulting  maneuver  can  be  identified  by  comparing  Sit) 
to  previous  non-maneuver  test  statistics  ^(t  —  At), Sit  —  2At), . .  .,^(t  —  mAt),  with  m  the  size  of 
a  moving  window.  Similarly,  if  the  type  of  maneuver  leads  to  the  disappearance  of  Ugy  in  uiy,  the 
scheme  also  reacts  instantaneously. 


13 


There  are  situations  in  which  the  attitude  change  due  to  climbing,  at  maneuver  instants,  is  less 
than  the  change  arising  from  pitch  motion.  On  these  occasions,  if  we  regard  u>sy{i)  the  desired 
signal,  and  0{t)  as  the  additive  noise,  we  are  dealing  with  a  problem  involving  a  small  signal  to  noise 
ratio.  The  test  statistic  6{t)  may  become  inappropriate  since  the  maneuver  test  statistic  does  not 
manifest  itself  in  larger  values.  We  therefore  impose  another  conservative  criterion.  This  criterion 
cannot  detect  maneuvers  instantaneously.  However,  it  notices  the  event  a  few  frames  later  and 
prevents  the  divergence  of  the  algorithm. 

specifically,  observe  that  when  the  vehicle  only  undergoes  pitch  motion,  p^i,  •  •  • ,  PmiV  are  likely 
to  oscillate  around  their  own  reference  positions.  The  conservative  detection  criterion  monitors  the 
vertical  movements  of  these  points  with  some  frequency  less  than  the  sequence  acquisition  rate. 
If  these  N  points  move  away  from  the  reference  positions,  the  algorithm  reports  the  occurrence 
of  climbing.  Reprocessing  a  small  set  of  images  is  then  performed.  Similarly,  when  the  smooth 
climbing  ends,  these  points  oscillate  around  new  reference  positions.  Appropriate  actions  are  then 
taken. 

We  now  proceed  to  address  the  separation  of  climbing  and  pitch  motions.  When  the  occurrence 
of  cHmbing  is  detected,  the  algorithm  responds  by  incorporating  different  dynamic  laws  for  selective 
stabilization.  When  the  scheme  detects  the  disappearance  of  climbing,  the  algorithm  concentrates 
on  obtaining  the  relevant  information  of  the  pitch  angle  0  using  the  estimated  u>y.  Let  the  vector 
y  consist  of  the  quantities  of  interest.  When  there  is  only  pitch  motion, 

y  =  i0,0f-,  (48) 

otherwise,  y  is  augmented  to  include  the  climbing  component: 

y=io:sy,e,0f  (49) 

Then,  as  discussed  in  Section  3,  by  using  the  dynamic  laws  in  (17)  and  (21),  climbing  and  pitch 
motion  can  be  separated.  To  take  into  account  the  noise  in  the  estimate  of  Uy,  we  apply  another 
Kalman  Filter  to  estimate  y.  Using  the  assumptions  that 

^  =  OJz 

we  have  therefore  separated  the  smooth  rotation  and  oscillatory  rotation.  The  next  section  discusses 
the  use  of  selective  stabilization  to  recover  the  3-D  locations  of  feature  points  which  are  close  to 
the  vehicle. 
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4.3  Structure  from  Selective  Stabilization 


Recall  that  the  motion  of  a  close  3-D  point  relative  to  the  stabilized  coordinate  system  Vs  is 
described  by 

Pv,  =  -cus  X  Pv,  -  Vs  (51) 

where  Py,  denotes  the  coordinates  of  the  point  in  Vs,  Ws  is  the  angular  velocity  due  to  the  smooth 
rotation,  and  Vs  is  the  vehicle’s  translational  velocity  under  no  disturbances.  When  the  vehicle 
undergoes  constant  smooth  rotation  and  translation  between  t  —  At  and  t,  we  have 

Pv,(t)  =  Rs(u»s;  t,  t  -  At)Pvs(i  -  At)  +  Ts(a»s;  t,  t  -  At)  (52) 


where  R*  accounts  for  the  resulting  attitude  change  as  explained  in  (25),  while  Ts  can  be  shown 
[21]  to  be 


Ts(ws;t,t-  At)  = 


lAt- 


1  -  cos(|a>s|At)  |a>s|At-  sin(|a;s|At) 


-f2s  + 


Vs 


(53) 


with  S2s  being  defined  in  (26)  and  |a>s|  denoting  the  magrdtude  of  Ws-  In  particular,  observing  that 
the  translational  movement  of  a  vehicle  is  along  its  longitudinal  axis  most  of  the  time  [12],  Vs  is 
simplified  to 

Vs  =  (0,0,us)^  (54) 


with  Vs  being  the  forward  speed.  Since  Vg  can  easily  be  obtained  from  the  odometer,  we  assume 
that  it  is  known.  Ts  is  then  simplified  accordingly. 

The  relative  motion  of  the  point  with  respect  to  the  unstabilized  coordinate  system  V  is  then 
obtained  as 

Pv(f)  =  R„.(^,  t)  {Pvs(f)  -  T,s(t)}  (55) 


where  T^s  is  due  to  the  bounce  of  the  vehicle, 


Tus(i)  =  (a;c(i)»05  0)^ 


(56) 


The  projection  of  the  3-D  point  on  the  image  is  then  computed  by  applying  the  perspective  pro¬ 
jection  operator  V: 

p  =  ^(Pv  -  d)  +  Pc  (57) 

where  d  specifies  the  position  of  the  camera  with  respect  to  V;  it  remains  constant  because  the 
camera  is  mounted  rigidly  on  the  vehicle.  Since  the  effect  of  T„s  is  usually  too  small  to  be  observed 
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from  the  images,  it  is  not  considered.  Therefore,  by  tracking  the  points  of  interest  over  the  sequence, 
as  well  as  using  the  estimates  from  selective  stabilization,  we  again  obtain  a  system  suitable  for 
recursive  estimators  such  as  EKF.  The  corresponding  plant  equations  for  Mg  such  points  are 

PVs,i(^j+i)  ~  ^-5(^55 "h  Ts(ctJ3;  tj+i, tj)  j  —  1  •  •  'Ms  (5^) 

The  measurement  equations,  on  the  other  hand,  are 

P^.  =  7>(Pvj  -  d)  +  Pc  j  =  l---Ms  (59) 

As  seen  from  (58),  close  features  are  estimated  in  the  stabilized  frame  of  reference.  The  undesired 
oscillatory  phenomena  seen  from  the  unstabilized  coordinate  system  are  removed. 

This  concludes  our  scheme  for  off-road  navigation  using  selective  stabilization.  The  next  section 
presents  simulation  results. 

5  Synthetic  Experiments 

In  this  section  we  illustrate  the  proposed  approach  through  synthetic  experiments.  Two  experiments 
were  performed.  One  corresponds  to  the  situation  in  which  the  attitude  change  due  to  climbing 
is  much  larger  than  the  change  arising  from  pitch  motion,  the  so-called  high  signal  to  noise  ratio 
case.  The  other  one  deals  with  the  case  involving  low  signal  to  noise  ratio. 

Consider  the  scenario  in  which  the  vehicle  undergoes  steering  for  the  first  one  second,  with 
a  rate  of  =  0.1  rad  •  s“^.  Immediately  after  the  steering,  the  vehicle  starts  climbing  over  a 
hiU.  The  smooth  rotation  due  to  climbing  exists  for  two  seconds.  The  cUmbing  rates  are  set  to 
LJsy  =  0.1  rad-s“^  and  0.01  rad-s“^,  in  the  high  and  low  signal  to  noise  ratio  situations  respectively. 
Finally,  the  vehicle  moves  uphill  along  a  straight  path  for  the  last  two  seconds,  i.e.  no  smooth 
rotation  occurs.  The  forward  speed  of  the  vehicle  is  maintained  at  Vs  —  10.0m  •  s“^. 

The  surface  irregularities,  on  the  other  hand,  are  modeled  using  first  order  Markov  processes; 
their  coefficients  depend  on  the  terrain  roughness  and  the  vehicle’s  forward  speed.  Therefore,  the 
excitations  to  the  vehicle  are  given  by 

ioi{t)  =  -avxoi{t)  -f  nav{t)  i  =  1  •  •  -4  (60) 

where  Uav  is  the  white,  Gaussian  driving  noise  with  mean  zero  and  variance  set  to  2a‘^av.  The  values 
of  a  and  cr,  in  addition,  vary  for  different  surfaces.  We  choose  a  =  0.012m  and  a  =  0.8m  ^  [15]. 
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The  nominal  values  of  the  vehicle’s  parameters  are  listed  in  Table  1.  According  to  the  equations  of 
motion  derived  in  Appendix  A,  we  synthesize  the  oscillatory  behavior  of  the  vehicle  and  illustrate 
it  in  Figure  4  (only  pitch  and  roll  phenomena  are  shown).  Together  with  the  smooth  motion,  the 
unstabilized  behavior  of  the  vehicle  is  obtained. 


Table  1:  The  nominal  parameter  values  of  the  four-wheel  vehicle  model. 


Mb 

^yy 

J-ZZ 

1710.0kg 

1031.3kg- m^ 

201.8kg -m'-^ 

57.5kg 

M^r 

Kt 

Cf  {Cr) 

Kf 

75.0kg 

200.0  kN  •  m"^ 

1.0  kN  •  m“^  •  s“^ 

18.0  kN-m-^ 

Kr 

'I's.r  {Ts.l) 

Wa 

Wb 

10.0  kN  •  m-^ 

0.6m 

1.4m 

1.3m 

(a) 


(b) 


Figure  4:  An  example  of  the  pitch  (solid  line)  and  roll  (dashed  line)  behavior  of  the  nominal  vehicle, 
(a)  0{t)  and  <j>{t),  (b)  9{t)  and  ^{t). 


In  addition,  during  driving,  a  sequence  of  images  is  acquired  at  20  Hz  from  an  on-board  camera 
(each  image  has  size  2.0  x  2.0  and  resolution  2000  X  2000),  while  a  set  of  features  including  eight 
distant  points  and  four  close  points  are  tracked.  The  coordinates  of  these  points  with  respect  to 
the  initial  frame  of  reference  are  listed  in  Table  2. 

The  algorithm  is  now  applied  to  estimate  the  total  angular  velocity,  to  separate  the  rotation 
due  to  climbing  and  pitch  motion,  as  well  as  to  obtain  the  3-D  locations  of  four  close  features  with 
respect  to  the  stabilized  coordinate  system. 
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Table  2:  The  initial  3-D  locations  of  the  feature  points:  the  first  eight  points  are  regarded  as  distant 
features,  whUe  the  last  four  are  considered  to  be  close  features. 


Point 

3-D  coordinates 

Point 

3-D  coordinates 

Point 

3-D  coordinates 

1 

1260 

36 

1800 

5 

1620 

1440 

1800 

9 

45.5 

-45.5 

65.0 

2 

1600 

-20 

2000 

6 

1400 

-1400 

2000 

10 

46.4 

-23.2 

58.0 

3 

-1840 

-69 

2300 

7 

1150 

-920 

2300 

11 

45.0 

30.0 

60.0 

4 

-1575 

70 

1750 

8 

875 

875 

1750 

12 

42.0 

56.0 

70.0 

5.1  High  Signal  to  Noise  Ratio 

We  consider  here  the  situation  in  which  the  vehicle  climbs  uphill  with  a  high  climbing  rate,  oJgy  = 
0.1  rad  •  s”^ .  The  performance  of  the  algorithm  is  evaluated  from  50  Monte  Carlo  trials.  The  total 
angular  velocity  is  first  computed  from  eight  listed  distant  points.  The  resulting  x  and  z  components 
are  directly  regarded  as  the  steering  rate  Ugx  and  the  roll  velocity  <^.  Figures  5  and  7  respectively 
show  the  corresponding  bias  and  root  mean-square  errors  of  the  two  estimates.  The  estimate  of 
Uy  is  illustrated  in  Figure  6a.  Meanwhile,  to  detect  the  occurrence  of  climbing,  the  test  statistic 
6  is  calculated  by  considering  the  vertical  movements  of  points  1—4,  and  is  shown  in  Figure  6b. 
As  seen  from  Figure  6b,  6  exhibits  large  values  when  maneuvers  occur  (t  =  1.0  s  and  t  =  3.0  s). 
Thus  maneuvers  can  be  detected  instantaneously.  We  thereupon  take  Usy  iato  consideration  from 
t  =  1.0  s  to  t  =  3.0  s.  The  results  of  selective  stabilization  are  shown  in  Figure  8.  It  can  be  seen 
that  the  algorithm  separates  chmbing  and  pitch  phenomena  quite  web.  After  selective  stabilization 
has  been  achieved,  these  estimates  are  used  in  obtaining  3-D  information  about  close  features,  i.e. 
points  9-12.  The  initial  guesses  about  these  points  are  listed  in  Table  3,  while  the  estimates  are 
shown  in  Figure  9.  The  respective  estimates  exhibit  similar  behaviors;  therefore  only  the  results 
for  the  12*^  point  are  displayed.  It  is  observed  that,  when  the  signal  to  noise  ratio  is  high,  the 
algorithm  provides  satisfactory  estimates  which  should  be  useful  for  off-road  navigation. 

Table  3:  Initial  guesses  of  the  3-D  locations  of  points  9-12. 


Point 

3-D 

coordinates 

9 

42.3 

-42.9 

60.0 

10 

40.4 

-20.5 

50.0 

11 

37.9 

25.6 

50.0 

12 

36.2 

48.9 

60.0 
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rimc(s)  Timc(s) 

(a)  (b) 

Figure  5:  Estimates  of  the  steering  rate  Usx,  ia  the  high  signal  to  noise  ratio  case,  from  50  Monte 
Carlo  trials,  (a)  True  (dashed  line)  and  estimated  (solid  line),  (b)  root  mean-square  error. 


(a)  (b) 

Figure  6:  Estimates  of  the  climbing- pitch  rate  Uy  and  test  statistic  6,  in  the  high  signal  to  noise 
ratio  case,  from  50  Monte  Carlo  trials,  (a)  ojy,  (b)  6. 

5.2  Low  Signal  to  Noise  Ratio 

We  now  present  some  results  corresponding  to  the  low  signal  to  noise  ratio  case,  i.e.  the  vehicle 
undergoes  climbing  at  a  small  rate,  u^y  =  0.01  rad  •  s~^.  Again,  the  performance  is  evaluated  from 
50  Monte  Carlo  trials.  Figures  10a  and  10b  show  the  estimates  of  the  total  angular  velocity  a? 
and  the  test  statistic  (5.  Since  the  a;  and  z  components  of  a?  exhibit  behavior  similar  to  that  in 
the  high  signal  to  noise  ratio  case,  they  are  not  displayed  here.  On  the  other  hand,  contrast  with 
Figure  6b,  the  test  statistic  6  does  not  manifest  large  values  at  t  =  1.0  s  and  t  =  3.0  s.  The 
occurrence  and  disappearance  of  climbing  are  therefore  not  detected  instantaneously.  However,  by 
monitoring  the  vertical  movements  of  the  first  four  distant  points  at  the  rate  of  2  Hz,  shown  in 


19 


(c)  (a) 

Figure  7:  Estimates  of  roll-related  parameters,  in  the  high  signal  to  noise  ratio  case,  from  50  Monte 
Carlo  trials,  (a)  <f>:  true  (dashed  line)  and  estimated  (solid  line),  (b)  4>:  root  mean-square  error,  (c) 
(/>:  true  (dashed  line)  and  estimate  (solid  line),  (d)  root  mean-square  error. 

Figure  11,  the  scheme  observes  that  these  points  move  away  from  their  first  reference  positions  at 
t  =  2.5  s.  Consequently,  the  algorithm  reprocesses  the  sequence  from  the  image  acquired  at  t  =  0.5 
s.  Similarly,  after  t  =  3.5  s,  these  points  oscillate  around  new  reference  positions.  The  scheme 
declares  the  instant  the  climbing  disappears  to  be  t  =  3.5  s.  Based  on  these  conservative  decisions, 
the  selective  stabilization  results  are  shown  in  Figure  13.  As  seen  from  this  figure,  the  estimate 
of  (x>sy  is  noisier  in  this  case.  This  leads  to  degradation  in  the  estimates  of  6  and  6.  The  results 
corresponding  to  the  3-D  locations  of  close  feature  points  are  shown  in  Figure  12.  They  exhibit  a 
slightly  larger  bias.  This  is  expected  since  the  orientation  of  the  stabilized  coordinate  system  V*  is 
biased  because  of  the  additional  smooth  rotation  that  presumably  occurred  between  t  =  3.0  s  and 
t  =  3.5  s.  Irrespective  of  these  relatively  larger  biases,  the  corresponding  absolute  errors  remain 
small.  The  structure  information  is  therefore  more  or  less  preserved. 
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Monte  Carlo  trials,  (a)  Usy*.  true  (dashed  line)  and  estimated  (solid  line),  (b)  ujsy’  root  mean-square 
error,  (c)  6:  true  (dashed  line)  and  estimated  (solid  line),  (d)  0:  root  mean-square  error,  (e)  k  true 
(dashed  line)  and  estimated  (solid  line),  (f)  6:  root  mean-square  error. 


(a)  (b) 

Figure  9:  Bias  and  root  mean-square  error  in  the  estimates  of  3-D  locations  of  close  features,  in 
the  high  signal  to  noise  ratio  case,  from  50  Monte  Carlo  trials  (solid  hne  corresponds  to  the  x 
coordinate,  dashed  line  to  the  y  coordinate,  and  dashed-dot  line  to  the  z  coordinate),  (a)  Bias  in 
Point  12,  (b)  root  mean-square  error  in  Point  12. 


(a)  (b) 

Figure  10:  Estimate  of  the  climbing-pitch  rate  Uy  and  test  statistic  in  the  low  signal  to  noise 
ratio  case,  from  50  Monte  Carlo  trials,  (a)  (b)  6. 

6  Conclusions 

This  paper  has  presented  a  scheme  for  estimating  the  motion  of  a  mobile  robot  as  well  as  the  relative 
positions  of  a  set  of  discrete  features.  In  particular,  the  algorithm  employs  different  dynamic  laws  to 
capture  different  phenomena  and  utilizes  various  visual  cues.  It  is  shown  that  selective  stabilization 
is  possible.  The  use  of  an  approximate  kinetic  law  in  achieving  selective  stabilization  is  justified. 
Schemes  for  the  detection  of  the  occurrence  and  disappearance  of  smooth  rotation  are  investigated. 
Lastly,  due  to  the  separation  of  smooth  rotation  and  oscillatory  rotation,  the  3-D  locations  of  close 
features  are  easily  estimated  in  a  less  perturbed  frame  of  reference. 
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Figure  11:  Vertical  movements  of  distant  points  when  the  signal  to  noise  ratio  is  low.  Points  1  and 
2  are  displayed. 


(a)  (b) 

Figure  12:  Bias  and  root  mean-square  error  in  the  estimates  of  the  3-D  locations  of  close  features, 
in  the  low  signal  to  noise  ratio  case,  from  50  Monte  Carlo  trials  (solid  line  corresponds  to  the  x 
coordinate,  dashed  line  to  the  y  coordinate,  and  dashed-dot  line  to  the  z  coordinate),  (a)  Bias  in 
Point  12,  (b)  root  mean-square  error  in  Point  12. 
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Figure  13:  Estimates  of  climbing-pitch  parameters,  in  the  low  signal  to  noise  ratio  case,  from  50 
Monte  Carlo  trials,  (a)  u^y'-  true  (dashed  line)  and  estimated  (solid  line),  (b)  Usy-  root  mean-square 
error,  (c)  0:  true  (dashed  line)  and  estimated  (soUd  Une),  (d)  6:  root  mean-square  error,  (e)  6:  true 
(dashed  Une)  and  estimated  (soUd  Une),  (f)  6:  root  mean-square  error. 


References 


[1]  G.  Adiv.  Determining  three-dimensional  motion  and  structure  from  optical  flow  generated  by 
several  moving  objects.  IEEE  Trans,  on  Pattern  Analysis  and  Machine  Intelliyence^  7.384—401, 
July  1985. 

[2]  J.K.  Aggarwal.  Motion  and  time-varying  imagery— An  overview.  In  Proc.  of  IEEE  Workshop 
on  Motion:  Representation  and  Analysis,  pages  1-6,  Kiawah  Island,  SC,  May  1986. 

[3]  P.  Burt,  R.  Hingorani,  and  R.J.  Kolczynski.  Mechanisms  for  isolating  component  patterns  in 
the  sequential  analysis  of  multiple  motion.  In  Proc.  of  IEEE  Workshop  on  Visual  Motion, 
pages  187-193,  Princeton,  NJ,  October  1991. 

[4]  C.T.  Chen.  Linear  System  Theory  and  Design.  Holt,  Rinehart,  and  Winston,  New  York,  1984. 

[5]  E.A.  Desloge.  Classical  Mechanics,  volume  2.  New  York:  John  Wiley  k  Sons,  1982. 

[6]  J.Q.  Fang  and  T.S.  Huang.  Some  experiments  on  estimating  the  3-D  motion  parameters  of  a 
rigid  body  from  two  consecutive  image  frames.  IEEE  Trans,  on  Pattern  Analysis  and  Machine 
Intelligence,  6:545-554,  September  1984. 

[7]  A.  Hady  and  D.A.  Crolla.  Theoretical  analysis  of  active  suspension  performance  using  a  four- 
wheel  vehicle  model.  Proc.  of  the  Institution  of  Mechanical  Engineers,  203:125-135, 1989. 

[8]  D.J.  Heeger.  Optical  flow  using  spatiotemporal  filters.  International  Journal  of  Computer 
Vision,  1:279-302, 1988. 

[9]  J.  Heel.  Direct  estimation  of  structure  and  motion  from  multiple  frames.  AI  Memo  1190,  MIT 
Artificial  Intelligence  Laboratory,  March  1990. 

[10]  B.K.P.  Horn  and  B.G.  Schunck.  Determining  optical  flow.  Artificial  Intelligence,  17:185-203, 
August  1981. 

[11]  J.K.  Kearney,  W.B.  Thompson,  and  D.L.  Boley.  Optical  flow  estimation:  An  error  analysis 
of  gradient-based  methods  with  local  optimization.  IEEE  Trans,  on  Pattern  Analysis  and 
Machine  Intelligence,  9:229-244, 1987. 

[12]  Y.  Liu  and  T.S.  Huang.  Vehicle-type  motion  estimation  from  multi-frame  images.  IEEE  Trans, 
on  Pattern  Analysis  and  Machine  Intelligence,  15:802-808,  August  1993. 


25 


[13]  H.C.  Longuet-Higgins.  The  reconstruction  of  a  scene  from  two  projections— Configurations 
that  defeat  the  8-point  algorithm.  In  Proc.  of  IEEE  Conf.  on  Artificial  Intelligence  Applica¬ 
tions,  pages  395-397,  Denver,  CO,  December  1984. 

[14]  H.C.  Longuet-Higgins  and  K.  Prazdny.  The  interpretation  of  a  moving  retinal  image.  Proc. 
Royal  Society  London,  pages  385-397, 1980. 

[15]  P.  Michelberger,  L.  Palkovics,  and  J.  Bokor.  Robust  design  of  active  suspension  system. 
International  Journal  of  Vehicle  Design,  14:145-165, 1993. 

[16]  H.H.  Nagel  and  W.  Enkelmann.  An  investigation  of  smoothness  constraints  for  the  estimation 
of  displacement  vector  fields  from  image  sequences.  IEEE  Trans,  on  Pattern  Analysis  and 
Machine  Intelligence,  8:565-593,  1986. 

[17]  D.  Sinclair,  A.  Blake,  and  D.  Murray.  Robust  estimation  of  egomotion  from  normal  flow. 
International  Journal  of  Computer  Vision,  13:57-69,  September  1994. 

[18]  R.Y.  Tsai  and  T.S.  Huang.  Estimating  three-dimensional  motion  parameters  of  a  rigid  planar 
patch  iii:  Finite  point  correspondences  and  the  three  view  problem.  IEEE  Trans,  on  Acoustics, 
Speech,  and  Signal  Processing,  32:213—220,  April  1984. 

[19]  A.M.  Waxman,  B.  Kamgar-Parsi,  and  M.  Subbarao.  Closed-form  solutions  to  image  flow 
equations  for  3D  structure  and  motion.  International  Journal  of  Computer  Vision,  1:239-258, 
1987. 

[20]  Y.S.  Yao,  P.  Burlina,  R.  Chellappa,  and  T.H.  Wu.  Electronic  image  stabilization  using  multiple 
visual  cues.  In  Proc.  of  IEEE  International  Conference  of  Image  Processing,  volume  1,  pages 
191-194,  Washington,  DC,  1995. 

[21]  Z.  Zhang  and  O.D.  Faugeras.  Three-dimensional  motion  computation  and  object  segmentation 
in  a  long  sequence  of  stereo  frames.  International  Journal  of  Computer  Vision,  7:211—241, 
August  1992. 


26 


Appendix  A 


We  apply  Lagrange’s  equations  of  motion  to  derive  the  oscillatory  behavior  of  the  four-wheel  vehicle 
model  in  this  appendix. 

Consider  Figures  2  and  3.  Define  the  generalized  coordinates,  consisting  of  seven  degrees  of 
freedom,  as 

q^=\xi,X3,X5,X7,Xc,0,<i>)'^  (61) 


Then  Lagrange’s  equations  of  motion  are  written  as  follows  [5]: 


dt  j  dqj  dqj  dqj 


Qj  j  —  l>  ■  ■  •  i  7 


where  j  =  1, . . . ,  7}  constitute  the  generalized  coordinates,  T  is  the  kinetic  energy  of  the  system, 
U  is  the  potential  energy,  D  is  the  dissipation  function  and  Qj's  represent  the  generalized  forces 
acting  on  the  system  during  vibration.  In  particular,  as  regards  the  behavior  of  XoO^B,  if  the 
suspension  system  is  of  passive  type,  then  the  corresponding  generalized  force  is  assumed  to  be 
zero.  We  look  at  various  energy  terms  in  the  following  paragraphs. 

•  Kinetic  energy 

It  is  well  known  that  the  kinetic  energy  of  a  moving  rigid  body  is  equal  to  the  sum  of  the 
translational  and  rotational  kinetic  energy.  Therefore,  taking  into  account  the  movements  of  the 
unsprung  elements  and  oscillations  of  the  sprung  element,  the  kinetic  energy  is 


T  =  Tb  + 


(63) 


where  Tb  is  the  kinetic  energy  contributed  by  the  sprung  element: 

Tb  = 


(64) 


with  Mb  denoting  the  mass  of  the  sprung  element,  and  lyy  and  Izz  being  the  moments  of  inertia 
with  respect  to  the  pitch  and  roll  axes.  Tyj,  on  the  other  hand,  is  the  kinetic  energy  contributed  by 
the  unsprung  elements 


T^  = 


(65) 
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•  Potential  energy 

The  potential  energy  U  takes  into  account  the  influences  of  suspension  springs  and  tires  on 
a  vehicle  under  going  vibration.  To  obtain  U,  the  deformations  of  suspension  springs  are  first 
computed.  They  are  approximated  by 


df,i  a  Xc  +  Ts,1<P+WaO-X3 

dr,r  ~  Ts,r4>  ~  WbO  —  X5 

dr, I  ^  Xc-¥  Ts,l<t>  —  WbO  -  X7 

Then  assunoing  that  linear  relationships  hold  between  the  deformations  and  the  forces  generated 
by  the  springs,  the  potential  energy  is 


U  =  Us +  Ut 


(67) 


where  Us  is  the  energy  stored  in  the  suspension  systems 

i/s  =  2^S,rd'),r  +  +  2^T,rdlj  +  -Kr,ldr,T 

and  Ut  is  the  energy  stored  in  the  tires, 

Ut  —  \^T  [(^^i  —  ®oi)^  +  ~  2^02)^  +  ~  2:03)^  +  (®7  —  2:04)^ 


(68) 


(69) 


•  Dissipation  function 

The  dissipation  function  accounts  for  the  effects  of  the  shock  absorbers.  Assuming  that  the 
forces  generated  by  the  shock  absorbers  vary  linearly  with  the  deforming  rate,  the  dissipation 
function  D  is 

D  =  2^Cf,rd^fr  +  2^f,ld}l  +  +  2i^r,ldrl  (70) 

After  various  energy  terms  have  been  considered,  (62)  is  applied.  The  dynamics  of  9  and  (j)  are 
then  obtained. 
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Appendix  B 


In  this  appendix  we  analytically  compare  the  performance  of  the  approximate  dynamic  law  of  pitch 
and  roU  motion  in  (21)  to  the  more  complete  dynamic  law  in  (19).  Equivalently,  assume  that  the 
smooth  rotation  and  oscillatory  rotation  are  described  by 

xo  =  Axo  +  Bu  US  (71) 


where  xq  =  {usy,(^sz,->^Lf ,  A  and  C  are  defined  in  (31)  and  (32)  respectively,  whfie 


B  = 


02x2  02x10 

04x2  Tus 


(72) 


with  r„s  mentioned  in  (19).  We  study  the  behavior  of  the  following  linear  systems. 

xi  =  Axi 
051  =  Cxi 


(73) 


where  Xi  consists  of  parameters  related  to  smooth  and  oscillatory  rotation,  as  defined  in  (30),  and 
a?i  denotes  the  measurable  total  angular  velocity,  again  as  defined  in  (30). 

We  employ  a  prediction- correction  based  scheme  to  separate  the  smooth  and  oscillatory  rotation 
[4]: 

xi  =  Axi  +  Ki[o5i  -  Cxi]  (74) 


The  resulting  error  xi  =  xo  -  xi  then  satisfies 

xi  =  (A  -  KiC)xi  -I-  Buus 


(75) 


By  choosing  an  appropriate  Ki,  xi  will  remain  bounded  as  long  as  u„s  is  bounded.  Therefore, 
given  reliable  estimates  of  the  total  angular  velocity,  we  can  employ  the  approximate  dynamic  law 
(21)  to  achieve  selective  stabilization  without  much  degradation. 
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